import pybullet as p
import pybullet_data
import time

# 启动并配置
p.connect(p.GUI)
p.setGravity(0, 0, -10)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # 设置资源路径，用于加载官方提供的模型

# 创建地面
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0)

# 创建跷跷板底座（一个矮圆柱）
baseId = p.createCollisionShape(p.GEOM_CYLINDER, radius=1, height=0.2)
p.createMultiBody(0, baseId, basePosition=[0,0,0.1])

# 创建跷跷板木板（一个长薄盒子），注意我们让它初始就有一点倾斜！
seesawShape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[4, 0.5, 0.1])
seesawId = p.createMultiBody(1, seesawShape, basePosition=[0,0,0.5], baseOrientation=p.getQuaternionFromEuler([0.2, 0, 0])) # 绕X轴旋转0.2弧度

# 创建两个小球，放在木板两端
ballShape = p.createCollisionShape(p.GEOM_SPHERE, radius=0.3)
ball1 = p.createMultiBody(5, ballShape, basePosition=[-3, 0, 1.2]) # 重球在左边（低处）
ball2 = p.createMultiBody(1, ballShape, basePosition=[3, 0, 1.8])  # 轻球在右边（高处）

# 创建约束，将木板和底座用铰链连接起来（允许绕X轴旋转）
constraint_1 = p.createConstraint(seesawId, -1, -1, -1, p.JOINT_POINT2POINT, [0,0,0], [0,0,0], [0,0,0.5])
constraint_2 = p.createConstraint(seesawId, -1, -1, -1, p.JOINT_POINT2POINT, [0,0,0], [0,1,0], [0,1,0.5])
# 更改为铰链关节
# p.changeConstraint(constraint, jointType=p.JOINT_REVOLUTE, pivot=[0,0,0.5], axis=[0,1,0]) # 绕Y轴旋转

# 仿真循环
try:
    while True:
        p.stepSimulation()
        time.sleep(1./240.)
except KeyboardInterrupt:
    print("\nQuit!\n")
finally:
    p.disconnect()